module(..., package.seeall)

enable = false

-- 6050 板子反了....
is6050 = false

local i2cid = 2 -- i2cid

-- 如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
-- 如果接V3.3,则IIC地址为0X69(不包含最低位).
local MPU9250_ADDR = 0X68 -- MPU6500的器件IIC地址
local MPU6500_ID1  = 0X71 -- MPU6500的器件ID1
local MPU6500_ID2  = 0X73 -- MPU6500的器件ID2

local MPU6050_ID = 0X68 -- MPU6050的器件ID

-- MPU9250内部封装了一个AK8963磁力计,地址和ID如下:
local AK8963_ADDR = 0X0C -- AK8963的I2C地址
local AK8963_ID   = 0X48 -- AK8963的器件ID

-- AK8963的内部寄存器
local MAG_WIA = 0x00 -- AK8963的器件ID寄存器地址
local MAG_CNTL1 = 0X0A
local MAG_CNTL2 = 0X0B

local MAG_XOUT_L = 0X03
local MAG_XOUT_H = 0X04
local MAG_YOUT_L = 0X05
local MAG_YOUT_H = 0X06
local MAG_ZOUT_L = 0X07
local MAG_ZOUT_H = 0X08

-- MPU6500的内部寄存器
local MPU_SELF_TESTX_REG   = 0X0D -- 自检寄存器X
local MPU_SELF_TESTY_REG   = 0X0E -- 自检寄存器Y
local MPU_SELF_TESTZ_REG   = 0X0F -- 自检寄存器Z
local MPU_SELF_TESTA_REG   = 0X10 -- 自检寄存器A
local MPU_SAMPLE_RATE_REG  = 0X19 -- 采样频率分频器
local MPU_CFG_REG          = 0X1A -- 配置寄存器
local MPU_GYRO_CFG_REG     = 0X1B -- 陀螺仪配置寄存器
local MPU_ACCEL_CFG_REG    = 0X1C -- 加速度计配置寄存器
local MPU_MOTION_DET_REG   = 0X1F -- 运动检测阀值设置寄存器
local MPU_FIFO_EN_REG      = 0X23 -- FIFO使能寄存器
local MPU_I2CMST_CTRL_REG  = 0X24 -- IIC主机控制寄存器
local MPU_I2CSLV0_ADDR_REG = 0X25 -- IIC从机0器件地址寄存器
local MPU_I2CSLV0_REG      = 0X26 -- IIC从机0数据地址寄存器
local MPU_I2CSLV0_CTRL_REG = 0X27 -- IIC从机0控制寄存器
local MPU_I2CSLV1_ADDR_REG = 0X28 -- IIC从机1器件地址寄存器
local MPU_I2CSLV1_REG      = 0X29 -- IIC从机1数据地址寄存器
local MPU_I2CSLV1_CTRL_REG = 0X2A -- IIC从机1控制寄存器
local MPU_I2CSLV2_ADDR_REG = 0X2B -- IIC从机2器件地址寄存器
local MPU_I2CSLV2_REG      = 0X2C -- IIC从机2数据地址寄存器
local MPU_I2CSLV2_CTRL_REG = 0X2D -- IIC从机2控制寄存器
local MPU_I2CSLV3_ADDR_REG = 0X2E -- IIC从机3器件地址寄存器
local MPU_I2CSLV3_REG      = 0X2F -- IIC从机3数据地址寄存器
local MPU_I2CSLV3_CTRL_REG = 0X30 -- IIC从机3控制寄存器
local MPU_I2CSLV4_ADDR_REG = 0X31 -- IIC从机4器件地址寄存器
local MPU_I2CSLV4_REG      = 0X32 -- IIC从机4数据地址寄存器
local MPU_I2CSLV4_DO_REG   = 0X33 -- IIC从机4写数据寄存器
local MPU_I2CSLV4_CTRL_REG = 0X34 -- IIC从机4控制寄存器
local MPU_I2CSLV4_DI_REG   = 0X35 -- IIC从机4读数据寄存器

local MPU_I2CMST_STA_REG = 0X36 -- IIC主机状态寄存器
local MPU_INTBP_CFG_REG  = 0X37 -- 中断/ 旁路设置寄存器
local MPU_INT_EN_REG     = 0X38 -- 中断使能寄存器
local MPU_INT_STA_REG    = 0X3A -- 中断状态寄存器

local MPU_ACCEL_XOUTH_REG = 0X3B -- 加速度值,X轴高8位寄存器
local MPU_ACCEL_XOUTL_REG = 0X3C -- 加速度值,X轴低8位寄存器
local MPU_ACCEL_YOUTH_REG = 0X3D -- 加速度值,Y轴高8位寄存器
local MPU_ACCEL_YOUTL_REG = 0X3E -- 加速度值,Y轴低8位寄存器
local MPU_ACCEL_ZOUTH_REG = 0X3F -- 加速度值,Z轴高8位寄存器
local MPU_ACCEL_ZOUTL_REG = 0X40 -- 加速度值,Z轴低8位寄存器

local MPU_TEMP_OUTH_REG = 0X41 -- 温度值高八位寄存器
local MPU_TEMP_OUTL_REG = 0X42 -- 温度值低8位寄存器

local MPU_GYRO_XOUTH_REG = 0X43 -- 陀螺仪值,X轴高8位寄存器
local MPU_GYRO_XOUTL_REG = 0X44 -- 陀螺仪值,X轴低8位寄存器
local MPU_GYRO_YOUTH_REG = 0X45 -- 陀螺仪值,Y轴高8位寄存器
local MPU_GYRO_YOUTL_REG = 0X46 -- 陀螺仪值,Y轴低8位寄存器
local MPU_GYRO_ZOUTH_REG = 0X47 -- 陀螺仪值,Z轴高8位寄存器
local MPU_GYRO_ZOUTL_REG = 0X48 -- 陀螺仪值,Z轴低8位寄存器

local MPU_I2CSLV0_DO_REG = 0X63 -- IIC从机0数据寄存器
local MPU_I2CSLV1_DO_REG = 0X64 -- IIC从机1数据寄存器
local MPU_I2CSLV2_DO_REG = 0X65 -- IIC从机2数据寄存器
local MPU_I2CSLV3_DO_REG = 0X66 -- IIC从机3数据寄存器

local MPU_I2CMST_DELAY_REG = 0X67 -- IIC主机延时管理寄存器
local MPU_SIGPATH_RST_REG  = 0X68 -- 信号通道复位寄存器
local MPU_MDETECT_CTRL_REG = 0X69 -- 运动检测控制寄存器
local MPU_USER_CTRL_REG    = 0X6A -- 用户控制寄存器
local MPU_PWR_MGMT1_REG    = 0X6B -- 电源管理寄存器1
local MPU_PWR_MGMT2_REG    = 0X6C -- 电源管理寄存器2 
local MPU_FIFO_CNTH_REG    = 0X72 -- FIFO计数寄存器高八位
local MPU_FIFO_CNTL_REG    = 0X73 -- FIFO计数寄存器低八位
local MPU_FIFO_RW_REG      = 0X74 -- FIFO读写寄存器
local MPU_DEVICE_ID_REG    = 0X75 -- 器件ID寄存器

-- 设置MPU9250的数字低通滤波器
-- lpf:数字低通滤波频率(Hz)
function setLpf(lpf)
    local data = 0
    if lpf >= 188 then
        data = 1
    elseif lpf >= 98 then
        data = 2
    elseif lpf >= 42 then
        data = 3
    elseif lpf >= 20 then
        data = 4
    elseif lpf >= 10 then
        data = 5
    else
        data = 6
    end
    i2c.send(i2cid, MPU9250_ADDR, {MPU9250_ADDR, MPU_CFG_REG, data}) -- 设置数字低通滤波器 
end

function setRate(rate)
    if rate > 1000 then
        rate = 1000
    end
    if rate < 4 then
        rate = 4
    end
    data = 1000 / rate - 1
    i2c.send(i2cid, MPU9250_ADDR, {MPU_SAMPLE_RATE_REG, data}) -- 设置数字低通滤波器
    return setLpf(rate / 2) -- 自动设置LPF为采样率的一半
end

function readChar(i2cid, reg, val)
    i2c.send(i2cid, reg, val)
    local res = i2c.recv(i2cid, reg, 1) or ""
    local n = res:byte(1) or -1
    return n
end

function setGyroFsr(fsr)
    i2c.send(i2cid, MPU9250_ADDR, {MPU_GYRO_CFG_REG, fsr * 8}) -- 设置陀螺仪满量程范围 
end

function setAccelFsr(fsr)
    i2c.send(i2cid, MPU9250_ADDR, {MPU_ACCEL_CFG_REG, fsr * 8}) -- 设置加速度传感器满量程范围
end

function readLen(i2cid, add, reg, len)
    i2c.send(i2cid, add, reg)
    local raw = i2c.recv(i2cid, add, len)
    if not raw or #raw ~= len then
        return false
    end
    return raw
end

-- 摄氏度
function getTmp()
    if not enable then
        return
    end
    local raw = readLen(i2cid, MPU9250_ADDR, MPU_TEMP_OUTH_REG, 2)
    if not raw then
        return false
    end
    local _, val = pack.unpack(raw, ">H")
    return (21 + val / 333.87)
end

function getGyro()
    if not enable then
        return
    end
    local raw = readLen(i2cid, MPU9250_ADDR, MPU_GYRO_XOUTH_REG, 6)
    if not raw then
        return false
    end
    local _, gx, gy, gz = pack.unpack(raw, ">hhh")
    local a = 9.8 * 16 / 32768
    return gx * a, gy * a, gz * a
end

function getAccel(f)
    if not enable then
        return
    end
    local raw = readLen(i2cid, MPU9250_ADDR, MPU_ACCEL_XOUTH_REG, 6)
    if not raw then
        return false
    end
    local _, ax, ay, az = pack.unpack(raw, ">hhh")
    local sig = is6050 and -1 or 1
    local a = 2000 * 57.295779513 / 32768 *sig
    return ax * a, ay * a, az * a
end

function getMag()
    if not enable or (louSig == -1) then
        return
    end
    local raw = readLen(i2cid, AK8963_ADDR, MAG_XOUT_L, 6)
    if not raw then
        return false
    end
    local _, mx, my, mz = pack.unpack(raw, "<hhh")
    -- AK8963每次读完以后都需要重新设置为单次测量模式
    i2c.send(i2cid, AK8963_ADDR, {MAG_CNTL1, 0X11})
    return mx, my, mz
end

function init()
    if i2c.setup(i2cid, i2c.SLOW) ~= i2c.SLOW then
        -- 初始化失败
        log.error("testI2c.init", "fail")
        return
    end
    i2c.send(i2cid, MPU9250_ADDR, {MPU_PWR_MGMT1_REG, 0X80}) -- 复位MPU9250
    sys.wait(100) -- 延时100ms
    i2c.send(i2cid, MPU9250_ADDR, {MPU_PWR_MGMT1_REG, 0X00}) -- 唤醒MPU9250
    setGyroFsr(3) -- 陀螺仪传感器, ±2000dps
    setAccelFsr(3) -- 设置加速度传感器满量程范围 
    setRate(50) -- 设置采样率50Hz			 	 	
    i2c.send(i2cid, MPU9250_ADDR, {MPU_INT_EN_REG, 0X00}) -- 关闭所有中断
    i2c.send(i2cid, MPU9250_ADDR, {MPU_USER_CTRL_REG, 0X00}) -- I2C主模式关闭
    i2c.send(i2cid, MPU9250_ADDR, {MPU_FIFO_EN_REG, 0X00}) -- 关闭FIFO
    i2c.send(i2cid, MPU9250_ADDR, {MPU_INTBP_CFG_REG, 0X82}) -- INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
    res = readChar(i2cid, MPU9250_ADDR, MPU_DEVICE_ID_REG) -- 读取MPU6500的ID
    if res == MPU6500_ID1 or res == MPU6500_ID2 then
        i2c.send(i2cid, MPU9250_ADDR, {MPU_PWR_MGMT1_REG, 0X01}) -- 设置CLKSEL,PLL X轴为参考
        i2c.send(i2cid, MPU9250_ADDR, {MPU_PWR_MGMT2_REG, 0X00}) -- 加速度与陀螺仪都工作
        setRate(50) -- 设置采样率为50Hz 
        log.info("iic", "加速度与陀螺仪 OK")
        enable = true
    elseif res == MPU6050_ID then
        log.info("iic", "加速度与陀螺仪 6050 OK")
        is6050 = true
        enable = true
        return
    else
        log.info("iic", "加速度与陀螺仪异常!!!", res)
        return
    end

    local res = readChar(i2cid, AK8963_ADDR, MAG_WIA) -- 读取MPU6500的ID
    if res == AK8963_ID then
        i2c.send(i2cid, AK8963_ADDR, {MAG_CNTL1, 0X11}) -- INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
        log.info("iic", "磁力计 OK")
    else
        log.info("iic", "磁力计异常!!!")
    end
end
